from rclpy.node import Node
from std_msgs.msg import Int16
from sls_controller.sls_pxOffboard import PxOffboard
from sls_controller.sls_publicDefine import *
from px4_msgs.msg import VehicleStatus
from scipy.spatial.transform import Rotation as scipyR

class UAVResource():
    #sigular instance
    _instance = None
    def __new__(cls, *args, **kwargs):
        if not cls._instance:
            cls._instance = super().__new__(cls)
        return cls._instance
    
    def __init__(self, main_node: Node, idStr:str):
        self.main_node = main_node
        self.platform_interface = PxOffboard(main_node)
        self.vehicle_class = VehicleClass.UAV
        self.takeoff_altitude={"NED":None,"msl":None,"ell":None,"laser":None} #NED msl ellipsoid
        self.takeoff_NED_position = {"N":None,"E":None,"D":None}
        self.takeoff_GNSS_position = {"latitude":None,"longitude":None}
        self.takeoff_timestamp = None
        self._idStr = idStr
        self.init_peripheral()
    
    #all peripheral init here
    def init_peripheral(self) -> None:
        self.height_laser_init()

    #height_laser
    ##eight_laser init
    def height_laser_init(self) -> None:
        self.height_laser_altitude = None
        self.height_laser_subscriber = self.main_node.create_subscription(
            Int16,
            "/sls/sensors/height_laser/out/dist_measure",
            self.height_laser_callback,
            2,
        )
    
    ##height_laser callback
    def height_laser_callback(self,msg:Int16) -> None:
        if msg.data < 0:
            self.height_laser_altitude = int(msg.data)
        else:
            self.height_laser_altitude = float(msg.data)/1000 #msg的单位是mm转换成 单位 m


            
    #flying status
    ##is flying status
    def is_flying_status(self) -> bool:
        if self.platform_interface.vehicle_status.takeoff_time > 0:
            return True
        else:
            return False
    ##get takeoff time us
    def get_takeoff_time(self) -> float:
        return self.platform_interface.vehicle_status.takeoff_time
    
    ##get flying time ref: introduction to timestamp of px4 when using in ros2
    def get_flying_time(self) -> float:
        return self.platform_interface.vehicle_status.timestamp-self.takeoff_timestamp
    
    # idStr
    def get_idStr(self) -> str:
        return self._idStr



    #control
    ##set position or velocity control mode
    def set_position_control(self):
        self.platform_interface.set_offboard_control_mode(position=True)

    def set_velocity_control(self):       
        self.platform_interface.set_offboard_control_mode(position=False,velocity=True)
    
    ##set Vehicle position
    def set_vehicle_position(self,N:float,E:float,D:float,yaw:float|None):
        # if yaw is None:
        #     #euler from quaternion
        #     rotation = scipyR.from_quat(self.platform_interface.vehicle_odometry.q)
        #      # 以 D,N,E 顺序获取欧拉角
        #     euler_angles = rotation.as_euler('zxy', degrees=False)
        #     yaw = euler_angles[0]
        if yaw is None:
            yaw = self.get_vehicle_yaw()
        self.platform_interface.publish_trajectory_setpoint(position=[N,E,D],yaw=yaw)
    
    def set_vehicle_velocity(self,N:float,E:float,D:float,yaw:float|None):
        # if yaw is None:
        #     #euler from quaternion
        #     rotation = scipyR.from_quat(self.platform_interface.vehicle_odometry.q)
        #      # 以 D,N,E 顺序获取欧拉角
        #     euler_angles = rotation.as_euler('zxy', degrees=False)
        #     yaw = euler_angles[0]
        if yaw is None:
            yaw = self.get_vehicle_yaw()
        self.platform_interface.publish_trajectory_setpoint(velocity=[N,E,D],yaw=yaw)
    
    # VehicleStatus and Control
    ## arming
    ### arming_status
    def is_arming_status_armed(self) -> bool:
        if self.platform_interface.vehicle_status.arming_state == VehicleStatus.ARMING_STATE_ARMED:
            return True
        else:
            return False
    ### vehicle arm
    def set_arm(self):
        self.platform_interface.arm()
    
    ## nav_state
    ### nav_state_user_intention
    def get_nav_state_user_intention(self) -> int:
        return self.platform_interface.vehicle_status.nav_state_user_intention
    ### nav_state
    def get_nav_state(self) -> int:
        return self.platform_interface.vehicle_status.nav_state
    ### is_nav_state_offboard_mode
    def is_nav_state_offboard_mode(self) -> bool:
        if self.platform_interface.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD:
            return True
        else:
            return False
    
    ### nav_state_set_offboard_mode
    def set_nav_state_offboard_mode(self):
        self.platform_interface.start_offboard_control_mode()
        self.platform_interface.engage_offboard_mode()
    
    #GNSS Sensors
    def get_GNSS_position(self) -> list[str, float]:
        return {"latitude":self.platform_interface.vehicle_global_position.lat,"longitude":self.platform_interface.vehicle_global_position.lon}
    
    #Odometry
    ##NED Position
    def get_NED_position(self) -> list[str, float]:
        return {"N":float(self.platform_interface.vehicle_odometry.position[0]),"E":float(self.platform_interface.vehicle_odometry.position[1]),"D":float(self.platform_interface.vehicle_odometry.position[2])}
    ##NED velocity
    def get_NED_velocity(self) -> list[str, float]:
        return {"N":float(self.platform_interface.vehicle_odometry.velocity[0]),"E":float(self.platform_interface.vehicle_odometry.velocity[1]),"D":float(self.platform_interface.vehicle_odometry.velocity[2])}

    #lat lon and alt ref of local frame
    def get_local_ref_lat(self) -> float:
        return self.platform_interface.vehicle_local_position.ref_lat
    
    def get_local_ref_lon(self) -> float:
        return self.platform_interface.vehicle_local_position.ref_lon
    
    def get_local_ref_alt(self) -> float:
        return self.platform_interface.vehicle_local_position.ref_alt
    #Altitude
    ##odometry NED Altitude
    def get_NED_altitude(self) -> float:
        return float(self.platform_interface.vehicle_odometry.position[2])
    ## GNSS msl Altitude 
    def get_msl_altitude(self) -> float:
        return self.platform_interface.vehicle_global_position.alt
    
    ## GNSS ell Altitude
    def get_ell_altitude(self) -> float:
        return self.platform_interface.vehicle_global_position.alt_ellipsoid
    
    def get_laser_altitude(self) -> float | None:
        return self.height_laser_altitude #  need to do 
    ## record take off all Altitude
    def record_takeoff_altitude(self) -> None:
        self.takeoff_altitude["NED"] = self.get_NED_altitude()
        self.takeoff_altitude["msl"] = self.get_msl_altitude()
        self.takeoff_altitude["ell"] = self.get_ell_altitude()
        self.takeoff_altitude["laser"] = self.get_laser_altitude()
    
    ## plus record take off poisiton
    def record_takeoff_NED_position(self) -> None:
        self.takeoff_NED_position = self.get_NED_position()

    def record_takeoff_GNSS_position(self) -> None:
        self.takeoff_GNSS_position = self.get_GNSS_position()

    def get_takeoff_altitude(self) -> dict:
        return self.takeoff_altitude
    def get_takeoff_NED_position(self) -> dict:
        return self.takeoff_NED_position
    
    def get_takeoff_GNSS_position(self) -> dict:
        return self.takeoff_GNSS_position    
    
    def record_takeoff_timestamp(self) -> None:
        self.takeoff_timestamp = self.platform_interface.vehicle_status.timestamp

    def get_takeoff_time(self) -> float:
        return self.takeoff_timestamp
        
    
    ## get relative Altitude to takeoff Altitude
    def get_relative_altitude(self) -> dict:
        if self.takeoff_altitude["NED"] != None:
            relative_NED_altitude = self.takeoff_altitude["NED"] - self.get_NED_altitude()
        else:
            relative_NED_altitude = None
        
        if self.takeoff_altitude["msl"] != None:
            relative_msl_altitude =  self.get_msl_altitude() - self.takeoff_altitude["msl"]
        else:
            relative_msl_altitude = None
        
        if self.takeoff_altitude["ell"] != None:
            relative_ell_altitude = self.get_ell_altitude() - self.takeoff_altitude["ell"]
        else:
            relative_ell_altitude = None
        #if 是防止两个None型的数据做减法运算导致报错 如果没有高度激光传感器这项就是None
        if (self.get_laser_altitude() != None) and (self.takeoff_altitude["laser"] != None):
            relative_laser_altitude = self.get_laser_altitude() - self.takeoff_altitude["laser"]
        else:
            relative_laser_altitude = None
        return {"NED":relative_NED_altitude,"msl":relative_msl_altitude,"ell":relative_ell_altitude,"laser":relative_laser_altitude}

    ## get_q_FRD_REFTO_NED()
    def get_q_FRD_REFTO_NED(self) -> list:
        q_list:list[float] = []
        for num in self.platform_interface.vehicle_odometry.q:
            q_list.append(float(num))
        return q_list
    
    ## get yaw
    def get_vehicle_yaw(self) -> float | None:
        try:
            # 假设 self.platform_interface.vehicle_odometry.q 是 [w, x, y, z]
            quat = self.platform_interface.vehicle_odometry.q

            # 调整四元数顺序为 [x, y, z, w]
            quat_xyzw = [quat[1], quat[2], quat[3], quat[0]]

            # 将四元数转换为欧拉角
            euler_angles = scipyR.from_quat(quat_xyzw).as_euler('zyx', degrees=False)
            yaw = float(euler_angles[0])
        except Exception as e:
            print(f"{self.__class__.__name__}get_vehicle_yaw_error: {e}")
            yaw = None
        return yaw
     
    #others
    def get_main_timer_during(self) -> float:
        return ParamDefault.MAIN_TIMER_DURING